#include "callback.h"

int32_t cnt = 0;
void IRAM_ATTR TimerCallBack_Controler(void)
{
    BaseType_t xHigherPriorityTaskWoken = pdFAIL;
    // float temppp = 3.1415926*encoder_cnt;
    // step_input = (int)temppp;
    //if (en_mode == ENABLE)
    if (ENABLE)
    {
        // 闭环模式
        if (closedloop_mode == ENABLE)
        {
            // 读出编码器的角度位置值
            encoder_cnt = CaliTab.get(Encoder.ReadAngleHwCsYORO());
            // 读出计数器计数的外部step脉冲数
            step_input = StepperInput.GetIntputPulse();
            // 计算出电机设定位置
            motor_pos_set = step_input_offset + stepangle * step_input / 100;
            // 编码器溢出处理
            if (encoder_cnt - encoder_cnt_last > (ENCODER_CNT_PER_REV / 2))
            {
                motor_rev_cnt--;
                xSemaphoreGiveFromISR(zerosignal_sem, &xHigherPriorityTaskWoken);
            }
            else if (encoder_cnt - encoder_cnt_last < -(ENCODER_CNT_PER_REV / 2))
            {
                motor_rev_cnt++;
                xSemaphoreGiveFromISR(zerosignal_sem, &xHigherPriorityTaskWoken);
            }
            // 计算出当前电机实际位置
            motor_pos = encoder_cnt + ENCODER_CNT_PER_REV * motor_rev_cnt;
            // 计算位置误差
            motor_pos_err = motor_pos_set - motor_pos;
            // 误差限幅
            if (motor_pos_err > (ENCODER_CNT_PER_REV * 0.1))
            {
                motor_pos_err = (ENCODER_CNT_PER_REV * 0.1);
                LED.set(HIGH);
            }
            else if (motor_pos_err < -(ENCODER_CNT_PER_REV * 0.1))
            {
                motor_pos_err = -(ENCODER_CNT_PER_REV * 0.1);
                LED.set(HIGH);
            }
            else
            {
                LED.set(LOW);
            }
            // 积分项计算
            pid_iterm += ki * motor_pos_err / 32;
            // 积分限幅
            if (pid_iterm > CLOSED_LOOP_MODE_CURRENT_SUM_MAX)
            {
                pid_iterm = CLOSED_LOOP_MODE_CURRENT_SUM_MAX;
            }
            else if (pid_iterm < -CLOSED_LOOP_MODE_CURRENT_SUM_MAX)
            {
                pid_iterm = -CLOSED_LOOP_MODE_CURRENT_SUM_MAX;
            }
            // 微分项计算
            pid_dterm = LOWPASS_FILTERING_A * pid_dterm / 128 - LOWPASS_FILTERING_B * kd * (motor_pos - motor_pos_last) / 8;
            // PID三项计算值
            vector_current = (kp * motor_pos_err + pid_iterm + pid_dterm) / 128;
            //
            advance = (motor_pos - motor_pos_last) * 3;
            // 更新历史值
            encoder_cnt_last = encoder_cnt;
            motor_pos_last = motor_pos;
            // 以当前位置为基础，计算下一步的目标位置
            int32_t theta = encoder_cnt;
            if (vector_current > 0)
            {
                theta += ((ENCODER_CNT_PER_REV / MOTOR_STEPS_PER_REV) + advance); //电流矢量等于1.8度加上前馈角
            }
            else if (vector_current < 0)
            {
                theta -= ((ENCODER_CNT_PER_REV / MOTOR_STEPS_PER_REV) - advance);
                vector_current = -vector_current;
            }
            // 电流矢量最大值限制
            if (vector_current > CLOSED_LOOP_MODE_CURRENT_MAX)
            {
                vector_current = CLOSED_LOOP_MODE_CURRENT_MAX;
            }
            cnt++;
            Output(theta, vector_current);
        }
        // 开环模式
        else
        {
            // 读出计数器计数的外部step脉冲数
            step_input = StepperInput.GetIntputPulse();
            // 计算出电机设定位置
            motor_pos_set = step_input_offset + stepangle * step_input / 100;
            // 如果位置不变，开始进行半流模式倒计时
            if (motor_pos_set == motor_pos_set_last)
            {
                half_current_cnt++;
                if (half_current_cnt >= HALF_CURRENT_CNT_MAX)
                {
                    half_current_cnt = HALF_CURRENT_CNT_MAX;
                }
            }
            else
            {
                half_current_cnt = 0;
            }
            // 1s时间已到，进入半流模式
            if (half_current_cnt >= OPEN_LOOP_MODE_CURRENT_MAX)
            {
                Output(motor_pos_set, OPEN_LOOP_MODE_CURRENT_MAX / 2);
            }
            else
            {
                Output(motor_pos_set, OPEN_LOOP_MODE_CURRENT_MAX);
            }
            // 历史值更新
            motor_pos_set_last = motor_pos_set;
        }
    }
}

void ENN_IRQHandler(void)
{
    if (StepperInput.IsEnlable() == true)
    {
        // 编码器初始化
        encoder_cnt = CaliTab.get(Encoder.ReadAngleHwCsYORO());
        encoder_cnt_last = encoder_cnt;
        // 位置设定值初始化
        motor_pos_set = encoder_cnt;
        motor_pos_set_last = encoder_cnt;
        // 位置值初始化
        motor_pos = encoder_cnt;
        motor_pos_last = encoder_cnt;
        // 位置偏移初始化
        step_input_offset = encoder_cnt;
        // 圈数初始化
        motor_rev_cnt = 0;
    }
    else
    {
        CoilA.free();
        CoilB.free();
    }
}